// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2017 Intel Corporation. All Rights Reserved.

#include <iostream>
#include <iomanip>
#include <map>
#include <utility>
#include <vector>
#include <librealsense2/rs.hpp>
#include <algorithm>
#include <opencv2/opencv.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include "api_how_to.h"
#include "debug.h"

int main(int argc, char *argv[])
try
{
    // 0 确定相机配置
    const char *serial_no = "250122079356";
    const rs2_format color_format = RS2_FORMAT_RGB8;
    const rs2_format depth_format = RS2_FORMAT_Z16;
    const int width = 1280;
    const int height = 720;
    const int fps = 30;

    // 1 初始化设备
    rs2::device dev = how_to::get_a_realsense_device(serial_no);

    // 2 获取所有sensor和stram_profile
    // 2.1 声明配置变量
    rs2::stream_profile color_profile;
    rs2::sensor color_sensor;
    rs2::stream_profile depth_profile;
    rs2::sensor depth_sensor;

    // 2.2 遍历获取所有sensor和stram_profile和对应关系
    std::vector<rs2::sensor> sensors = dev.query_sensors();
    for (rs2::sensor sensor : sensors)
    {
        std::vector<rs2::stream_profile> stream_profiles = sensor.get_stream_profiles();
        for (rs2::stream_profile sp : stream_profiles)
        {
            // color 图像格式判断，获取正确配置
            if (!color_sensor && how_to::is_right_profile(sp, RS2_STREAM_COLOR, color_format, width, height, fps))
            {
                color_sensor = sensor;
                color_profile = sp;
            }

            // depth 图像格式判断，获取正确配置
            if (!depth_sensor && how_to::is_right_profile(sp, RS2_STREAM_DEPTH, depth_format, width, height, fps))
            {
                depth_sensor = sensor;
                depth_profile = sp;
            }
        }
    }

    // 3 开始获取图像
    rs2::frame color_frame, depth_frame;
    how_to::start_streaming_a_profile(color_sensor, color_profile, color_frame);
    how_to::start_streaming_a_profile(depth_sensor, depth_profile, depth_frame);

    // 4 等待图像对齐
    std::vector<rs2::frame> bundle;
    rs2::processing_block bundler([&](rs2::frame f, rs2::frame_source &src)
                                  {
            bundle.push_back(f);
            if (bundle.size() == 2)
            {
                auto fs = src.allocate_composite_frame(bundle);
                src.frame_ready(fs);
                bundle.clear();
            } });
    rs2::frame_queue q;
    bundler.start(q);

    bundler.invoke(depth_frame);
    bundler.invoke(color_frame);
    rs2::frameset fs = q.wait_for_frame();

    // 5 深度图对齐到彩色图
    rs2::align align_depth_to_color(RS2_STREAM_COLOR);
    rs2::frameset aligned_frames = align_depth_to_color.process(fs);
    cv::Mat color_image(cv::Size(1280, 720), CV_8UC3, (void *)aligned_frames.get_color_frame().get_data(), cv::Mat::AUTO_STEP);
    cv::Mat depth_image(cv::Size(1280, 720), CV_16UC1, (void *)aligned_frames.get_depth_frame().get_data(), cv::Mat::AUTO_STEP);
    std::cout << "aligned depth to color " << std::endl;

    // 6 获取内参
    float ppx, ppy, fx, fy;
    if(color_profile.is<rs2::video_stream_profile>())
    {
        rs2::video_stream_profile video_stream = color_profile.as<rs2::video_stream_profile>();
        rs2_intrinsics intrinsics = video_stream.get_intrinsics();
        ppx = intrinsics.ppx;
        ppy = intrinsics.ppy;
        fx = intrinsics.fx;
        fy = intrinsics.fy;
        std::cout << "got rgb intrinsics" << std::endl;
    }
    else
    {
        std::cout << "can not get intrinsics" << std::endl;
        return EXIT_FAILURE;
    }

    // 7 深度图和彩色图转换到pcl格式
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    std::cout << "init point cloud" << std::endl;
    for (size_t i = 0; i < height; i++)
    {
        for (size_t j = 0; j < width; j++)
        {
            ushort d = depth_image.ptr<ushort>(i)[j];
            if (d == 0)
            {
                continue;
            }
            
            pcl::PointXYZRGB point;
            point.z = double(d) / 1000;
            point.x = (j - ppx) * point.z / fx;
            point.y = (i - ppy) * point.z / fy;

            // 计算颜色值
            cv::Vec3b bgr = color_image.at<cv::Vec3b>(i, j);
            point.r = bgr[2];
            point.g = bgr[1];
            point.b = bgr[0];

            cloud->push_back(point);
        }
    }
    std::cout << "got pointcloud " << std::endl;

    // 8 点云显示
    pcl::visualization::PCLVisualizer viewer("Point Cloud Viewer");
    viewer.setBackgroundColor(255, 255, 255); // rgb
    viewer.addPointCloud(cloud, "cloud viewer");
    viewer.spin();

    return EXIT_SUCCESS;
}
catch (const rs2::error &e)
{
    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (const std::exception &e)
{
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
}